#include "CVStip.h"

CVStip::CVStip()
{
	this->K=0;
	this->threshold_enable=false;
	this->threshold=0.0;
}
		
void CVStip::init(int dimT,int dimX,int dimY,float K,float sigmaS,float sigmaT,bool threshold_enable,float threshold)
{
	gaussians.init(sigmaS,sigmaT,dimT,dimX,dimY);
	this->K=K;
	this->threshold_enable=threshold_enable;
	this->threshold=threshold;
	idOutput=0;
	mask_use=false;
}

void CVStip::push(const cv::Mat & frame)
{
	//on converti l'image en nvg		
	cv::Mat frame_nvg; 
	frame_nvg.create(frame.rows,frame.cols,CV_8UC1);		
	cvtColor(frame, frame_nvg, CV_RGB2GRAY);		
	frame_nvg.convertTo(frame_nvg32f,CV_32F,1/255.0);

	//on l'ajoute dans le buffer
	cv::Mat new_frame=frame_nvg32f.clone();
	bufferT.push_back(new_frame);
	while((int)bufferT.size()>gaussians.getDimT())
	{
		bufferT.pop_front();
	}
}

cv::Mat & CVStip::process()
{
	//si le buffer est plein on doit caculer les images:
	//d²I/dx²,d²I/dy²,d²I/dxdy,d²I/dydx
	//d²I/dt²,d²I/dtdx,d²I/dtdy
	//d²I/dxdT,d²I/dydT

	if(((int)bufferT.size()==gaussians.getDimT()) && (bufferT.size()>1))
	{
		//pour debug		
		//printf("Taille du buffer=%d Indice de l'img en cours=%d\n",(int)bufferT.size(),(int)bufferT.size()/2+1);		

		//L'image en cours de traitement est celle du milieu du buffer
		current_img=bufferT[(int)bufferT.size()/2+1].clone();			

		//Calcul de dI²/dx²
		cv::Mat dx2;
		dx2.create(bufferT.front().rows,bufferT.front().cols,CV_32FC1);
		cv::filter2D(current_img,dx2,-1, gaussians.get_dGdx2(),cv::Point(-1,-1));

		//Calcul de dI²/dy²
		cv::Mat dy2;
		dy2.create(bufferT.front().rows,bufferT.front().cols,CV_32FC1);
		cv::filter2D(current_img,dy2,-1, gaussians.get_dGdy2(),cv::Point(-1,-1));

		//Calcul de dI²/dxdy et dI²/dydx
		cv::Mat dxdy_dydx;
		dxdy_dydx.create(bufferT.front().rows,bufferT.front().cols,CV_32FC1);
		cv::filter2D(current_img,dxdy_dydx,-1, gaussians.get_dGdxdy(),cv::Point(-1,-1));

		//Calcul de d²I/dt² et dI/dt et dI/dxdt et dI/dydt
		cv::Mat dt2;
		dt2.create(bufferT.front().rows,bufferT.front().cols,CV_32FC1);
		dt2.setTo(0);
		cv::Mat dt;
		dt.create(bufferT.front().rows,bufferT.front().cols,CV_32FC1);
		dt.setTo(0);
		cv::Mat dxdt;
		dxdt.create(bufferT.front().rows,bufferT.front().cols,CV_32FC1);
		dxdt.setTo(0);
		cv::Mat dydt;
		dydt.create(bufferT.front().rows,bufferT.front().cols,CV_32FC1);
		dydt.setTo(0);

		for(int i=0;i<gaussians.getDimT();i++)
		{		
			dt2=dt2+bufferT[i]*(gaussians.get_dGdt2()(i,1));		
			dt=dt+bufferT[i]*(gaussians.get_dGdt()(i,1));
		}
		
		//Calcul de d²I/dtdx et d²I/dtdy
		cv::Mat dtdx_dxdt;
		cv::filter2D(dt,dtdx_dxdt,-1, gaussians.get_dGdx(),cv::Point(-1,-1));
		cv::Mat dtdy_dydt;
		cv::filter2D(dt,dtdy_dydt,-1, gaussians.get_dGdy(),cv::Point(-1,-1));


		//on calcul maintenant la saillance de la matrice : S=det M - k trace(M)^3
		//               |d²I/dx²  d²I/dxdy d²I/dxdt|
		//               |                          |   |a b c|
		// det(M(x,y,t))=|d²I/dydx d²I/dy²  d²I/dydt| = |b d e| = a.(df-e²)-b²f+2bce+c²d
		//               |                          |   |c e f|
		//               |d²I/dtdx d²I/dtdy d²I/dt² |
		
		cv::Mat_<float> saillance;		
		cv::Mat_<float> trace;
		
		trace=dx2+dy2+dt2;
		
		saillance=dx2.mul( (dy2.mul(dt2)) - (dtdy_dydt.mul(dtdy_dydt)) ) - (dxdy_dydx.mul(dxdy_dydx)).mul(dt2)+
							2.0*dxdy_dydx.mul(dtdx_dxdt).mul(dtdy_dydt) + dtdx_dxdt.mul(dtdx_dxdt).mul(dy2)- K *((trace.mul(trace)).mul(trace));
		
		//filtrage des résultats
		if(mask_use)
		{		
			cv::Mat buffer;
			buffer.setTo(0);
			saillance.copyTo(buffer,mask);
			saillance=buffer;
			mask_use=false;	
		}

		//les points clefs sont les extremums locaux de la saillance
		keys.clear();		
		for(int c=1; c<saillance.cols-1; c++)
		{
			for(int r=1; r<saillance.rows-1; r++)
			{
				if(saillance(r,c)>threshold)
				{
					//est-ce un extremum local? si oui on le met dans la liste des points clefs
					if((saillance(r,c)>=saillance(r,c+1)) &&
					   (saillance(r,c)>=saillance(r,c-1)) &&
					   (saillance(r,c)>=saillance(r+1,c)) &&
						 (saillance(r,c)>=saillance(r-1,c)))
						{
							cv::KeyPoint pt(c,r,10,0,saillance(r,c),0,0);
							keys.push_back(pt);
						}
				}
			}
		}
		
		//on converti la liste des points clefs en matrice
		cv::Mat_<float> keyspoints(keys.size(),7);
		keyspoints.setTo(0);

    for(int i=0;i<(int)keyspoints.rows;i++)
    {
      keyspoints(i,0)=keys[i].pt.x;
      keyspoints(i,1)=keys[i].pt.y;
      keyspoints(i,2)=keys[i].size;
      keyspoints(i,3)=keys[i].angle;
      keyspoints(i,4)=keys[i].response;
			keyspoints(i,5)=keys[i].octave;//octave
			keyspoints(i,6)=keys[i].class_id;//label
    }

		if(idOutput==0)result=saillance;
		else if(idOutput==1)result=dx2;
		else if(idOutput==2)result=dy2;
		else if(idOutput==3)result=dt2;
		else if(idOutput==4)result=dt;
		else if(idOutput==5)result=dxdy_dydx;
		else if(idOutput==6)result=dtdx_dxdt;
		else if(idOutput==7)result=dtdy_dydt;
		else {result=keyspoints;}
	}
	else
	{
		result.create(1,1,CV_32FC1);
		result.setTo(0);
	}
	
	return result;
}

void CVStip::setDimT(int dimT)
{
	gaussians.setDimT(dimT);
}

void CVStip::setDimX(int dimX)
{
	gaussians.setDimX(dimX);
}

void CVStip::setDimY(int dimY)
{
	gaussians.setDimY(dimY);
}

void CVStip::setSigmaT(float sigmaT)
{
	gaussians.setSigma_T(sigmaT);
}

void CVStip::setSigmaS(float sigmaS)
{
	gaussians.setSigma_S(sigmaS);
}

void CVStip::setK(float K)
{
	this->K=K;
}

void CVStip::setThreshold(bool threshold_enable,float threshold)
{
	this->threshold_enable=threshold_enable;
	this->threshold=threshold;
}

int CVStip::getDimT()
{
	return gaussians.getDimT();
}

int CVStip::getDimX()
{
	return gaussians.getDimX();
}

int CVStip::getDimY()
{
	return gaussians.getDimY();
} 

float CVStip::getSigmaT()
{
	return gaussians.getSigma_T();
}

float CVStip::getSigmaS()
{
	return gaussians.getSigma_S();
}

float CVStip::getK()
{
	return K;
}

bool CVStip::isThresholdEnabled()
{
	return threshold_enable;
}

float CVStip::getThreshold()
{
	return threshold;
}

void CVStip::setOutput(int id)
{
	idOutput=id;
}

void CVStip::setMask(const cv::Mat & mask)
{
	this->mask=mask.clone();
	mask_use=true;
}
